function [observations, observed_ids] = observation_model_laser(pose, obstacles, laser_range, laser_fov, num_beams, noise_std)
% 基于激光雷达的观测模型（提取特征点）
% 输入: pose-位姿, obstacles-障碍物, laser_range-最大距离, laser_fov-视场角
%       num_beams-光束数, noise_std-噪声标准差
% 输出: observations-观测[r;φ], observed_ids-特征ID

    % 模拟激光扫描（返回是否击中障碍物的标记）
    [scan, hit_flags] = simulate_laser_scan_simple(pose, obstacles, laser_range, laser_fov, num_beams, noise_std);
    
    % 计算扫描角度
    theta = pose(3);
    angle_min = theta - laser_fov/2;
    angle_max = theta + laser_fov/2;
    angles = linspace(angle_min, angle_max, num_beams);
    
    % 从扫描中提取特征点（只使用击中障碍物的点）
    [features, feature_ids] = extract_features_from_scan(scan, angles, 0.5, laser_range, hit_flags);
    
    % 转换为相对观测（相对于机器人坐标系）
    observations = [];
    observed_ids = [];
    
    for i = 1:size(features, 2)
        range = features(1, i);
        bearing_global = features(2, i);
        
        % 转换为相对方位
        bearing_relative = wrapToPi(bearing_global - pose(3));
        
        observations = [observations, [range; bearing_relative]];
        observed_ids = [observed_ids; feature_ids(i)];
    end
end

function [scan, hit_flags] = simulate_laser_scan_simple(pose, obstacles, max_range, fov, num_beams, noise_std)
% 简化的激光扫描仿真（改进版：标记是否击中障碍物）
    x = pose(1);
    y = pose(2);
    theta = pose(3);
    
    angle_min = theta - fov/2;
    angle_max = theta + fov/2;
    angles = linspace(angle_min, angle_max, num_beams);
    
    scan = zeros(num_beams, 1);
    hit_flags = false(num_beams, 1);  % 标记是否击中真实障碍物
    
    for i = 1:num_beams
        angle = angles(i);
        dx = cos(angle);
        dy = sin(angle);
        
        min_dist = max_range;
        hit_obstacle = false;
        
        % 检查所有障碍物
        for j = 1:size(obstacles, 1)
            obs = obstacles(j, :);
            dist = ray_box_intersection(x, y, dx, dy, obs);
            if dist > 0 && dist < min_dist
                min_dist = dist;
                hit_obstacle = true;  % 击中了障碍物
            end
        end
        
        % 添加噪声
        scan(i) = min_dist + randn() * noise_std;
        scan(i) = max(0.1, min(scan(i), max_range));
        
        % 只有真正击中障碍物才标记为有效
        % 如果添加噪声后仍然接近max_range，认为没有击中
        if hit_obstacle && scan(i) < max_range - 0.2
            hit_flags(i) = true;
        end
    end
end

function dist = ray_box_intersection(x0, y0, dx, dy, box)
    x_min = box(1);
    y_min = box(2);
    x_max = box(3);
    y_max = box(4);
    
    t_min = -inf;
    t_max = inf;
    
    if abs(dx) > 1e-6
        t1 = (x_min - x0) / dx;
        t2 = (x_max - x0) / dx;
        t_min = max(t_min, min(t1, t2));
        t_max = min(t_max, max(t1, t2));
    else
        if x0 < x_min || x0 > x_max
            dist = -1;
            return;
        end
    end
    
    if abs(dy) > 1e-6
        t1 = (y_min - y0) / dy;
        t2 = (y_max - y0) / dy;
        t_min = max(t_min, min(t1, t2));
        t_max = min(t_max, max(t1, t2));
    else
        if y0 < y_min || y0 > y_max
            dist = -1;
            return;
        end
    end
    
    if t_min > t_max || t_max < 0
        dist = -1;
    elseif t_min > 0
        dist = t_min;
    else
        dist = -1;
    end
end

